#!/usr/bin/env python
#coding=utf-8

import rospy
import string
# import sys

import numpy as np
import pandas as pd
import message_filters

from ant_msgs.msg import ControlCmd2 # 我要修改的消息
from time_process.msg import ControlCmd2_new
from gps_msgs.msg import Inspvax
#from ant_msgs.msg import VehicleState

import sys
#sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
#import torch
print("import torch successfully")


#此速度文件为三车仿真实验数据
df_news = pd.read_table('/home/seu/projects/littleAnt_ws_dhn/src/data_record/scripts/lzh_0-20_p.txt',header = None)
v_lead = df_news[0]
#v_ego = df_news[1]
#v_later = df_news[2]

i = 0

#起点坐标
ini_lon = 118.80870
ini_lat = 31.88803
starting = 0

#实现静态变量功能
def f():
	if not hasattr(f, 'x'):
		f.x = 0
	f.x+=1
	return f.x

def LittleAntCmdTopicCallback(ControlCmd2_new, Inspvax):
	global i
	global starting
	gps_lat = Inspvax.latitude
	gps_lon = Inspvax.longitude
	print("===================")
	print("gps_lat = ", gps_lat)
	print("gps_lon = ", gps_lon)
	print("===================")
	if (abs(gps_lon - ini_lon) < 0.00001 and abs(gps_lat - ini_lat) < 0.0001) or starting == 1 :
		starting = 1
		NewSpeedPub.set_gear = ControlCmd2_new.set_gear
		#速度替换
		print("replaceing!!!!!!!!!!!!!!!!!!!!")
		if i <= (len(v_lead) - 1):
			NewSpeedPub.set_speed = v_lead[i]*3.6
		else:
			NewSpeedPub.set_speed = ControlCmd2_new.set_speed
		NewSpeedPub.set_brake = ControlCmd2_new.set_brake
		NewSpeedPub.set_accelerate = ControlCmd2_new.set_accelerate
		NewSpeedPub.set_roadWheelAngle = ControlCmd2_new.set_roadWheelAngle
		NewSpeedPub.set_emergencyBrake = ControlCmd2_new.set_emergencyBrake
		PubSimSpeedCmd.publish(NewSpeedPub)
		i = f()
	else :
		NewSpeedPub.set_gear = ControlCmd2_new.set_gear
		NewSpeedPub.set_speed = 10
		NewSpeedPub.set_brake = ControlCmd2_new.set_brake
		NewSpeedPub.set_accelerate = ControlCmd2_new.set_accelerate
		NewSpeedPub.set_roadWheelAngle = ControlCmd2_new.set_roadWheelAngle
		NewSpeedPub.set_emergencyBrake = ControlCmd2_new.set_emergencyBrake
		PubSimSpeedCmd.publish(NewSpeedPub)

if __name__ == '__main__':
	rospy.init_node('speed_pub_node')
	RealSpeedCmd = ControlCmd2_new()
	NewSpeedPub = ControlCmd2()
	#订阅小蚂蚁路径跟踪生成的消息指令
	# LittleAntCmdTopic = rospy.get_param('little_ant_cmd', '/ControlCmd2')
	s1 = message_filters.Subscriber('/controlCmd2_new', ControlCmd2_new)
	s2 = message_filters.Subscriber('/gps',Inspvax)
	PublishNewCmd = message_filters.ApproximateTimeSynchronizer([s1, s2], 10, 1, allow_headerless=True)
	PublishNewCmd.registerCallback(LittleAntCmdTopicCallback)
	#订阅路径跟踪消息后,重新生成指令
	SpeedCmdTopic = rospy.get_param('sim__speed_cmd', '/SimSpeedCmd')
	PubSimSpeedCmd = rospy.Publisher(SpeedCmdTopic,ControlCmd2, queue_size=10)
	rospy.spin()
